#ifndef PIBOT_BRINGUP_VARIABLE_QUEUE_H_
#define PIBOT_BRINGUP_VARIABLE_QUEUE_H_

#include <string.h>
#include <stdio.h>

namespace pibot {

template <unsigned short MAX_SIZE = 256>
class VQueue : public Queue {
 public:
  VQueue();
  bool put(unsigned char ch);
  bool get(unsigned char& ch);

  unsigned short size();
  unsigned short max_size();

 private:
  unsigned char _buffer[MAX_SIZE];
  unsigned short _max_size;
  unsigned short _head;
  unsigned short _tail;
};

template <unsigned short MAX_SIZE>
VQueue<MAX_SIZE>::VQueue() {
  _max_size = MAX_SIZE;
  memset(_buffer, 0, MAX_SIZE);
  _head = 0;
  _tail = 0;
}

template <unsigned short MAX_SIZE>
unsigned short VQueue<MAX_SIZE>::size() {
  return (_tail + _max_size - _head) % _max_size;
}

template <unsigned short MAX_SIZE>
unsigned short VQueue<MAX_SIZE>::max_size() {
  return _max_size;
}

template <unsigned short MAX_SIZE>
bool VQueue<MAX_SIZE>::put(unsigned char ch) {
  if (_tail + 1 == _head) return false;

  _buffer[_tail++] = ch;

  if (_tail >= MAX_SIZE) _tail = 0;

  return true;
}

template <unsigned short MAX_SIZE>
bool VQueue<MAX_SIZE>::get(unsigned char& ch) {
  if (_head == _tail) return false;

  ch = _buffer[_head++];

  if (_head >= MAX_SIZE) _head = 0;

  return true;
}

}  // namespace pibot

#endif